#include "GPS/get_uav_location.hpp"

GetUavLocation::GetUavLocation(QObject *parent, QSerialPort *serialPort,mavlink_message_t mavlink_msg)
    : QObject(parent) {
  this->serialPort = serialPort;
  this->updateMavlinkMessage(mavlink_msg);
}

// 更新mavlink消息并发射信号
void GetUavLocation::updateMavlinkMessage(const mavlink_message_t& msg) {
  this->mavlink_msg = msg;
  
  if (this->mavlink_msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
    mavlink_global_position_int_t pos;
    mavlink_msg_global_position_int_decode(&this->mavlink_msg, &pos);

    // 解析经纬度（degE7 -> 度）
    double lon = pos.lon / 1e7;
    double lat = pos.lat / 1e7;

    // 解析高度（mm -> m）
    double altitude = pos.alt / 1000.0;
    
    // 解析航向（cdeg -> 度）
    double heading = pos.hdg / 100.0;

    // 发射位置更新信号，使用系统ID作为无人机ID
    emit positionUpdated(lat, lon, altitude, heading, this->mavlink_msg.sysid);
  }
}
